package edu.ntnu.controller;

import java.util.List;

import edu.wsu.KheperaSimulator.RobotController;

/**
 * Robot's movement
 */
public class Movement {

	public static final int SPEED = 9;
	
	private RobotController robot;
	private MapBuilder mapBuilder;
	
	public Movement(RobotController newRobot) {
		robot = newRobot;
		mapBuilder = MapBuilder.getInstance();
	}

	/**
	 * Checks whether robot can move forward or there is something in front of him
	 * @return	true if robot can move forward, false otherwise
	 */
	public boolean canMoveForward() {
		return (robot.getDistanceValue(1) < 1023 && robot.getDistanceValue(2) < 1023 &&
				  robot.getDistanceValue(3) < 1023 && robot.getDistanceValue(4) < 1023);
	}
	
	/**
	 * Moves robot forward if possible.
	 * @param distance		distance to move
	 * @param speed			speed of moving (1 - 9)
	 * @return				true if required distance was reached, false otherwise
	 */
	public boolean moveForward(int distance, int speed) {
		return moveForward(distance, speed, false);
	}
	
	/**
	 * Moves robot backward if possible.
	 * @param distance		distance to move
	 * @param speed			speed of moving (1 - 9)
	 * @return				true if required distance was reached, false otherwise
	 */
	public boolean moveBackward(int distance, int speed) {
		return moveForward(distance * -1, speed, true);
	}
	
	/**
	 * Moves robot forward.
	 * @param distance		distance to move
	 * @param speed			speed of moving (1 - 9)
	 * @param forceMove		if true, robot doesn't care about wall in front of him
	 * @return				true if required distance was reached, false otherwise
	 */
	public boolean moveForward(int distance, int speed, boolean forceMove) {

		// check given parameters
		// TODO: change exception class
		if(speed < 1 || speed > 9) {
			throw new IllegalArgumentException("Speed " + speed + " is not allowed");
		}
		
		long initialLeftWheelPosition = robot.getLeftWheelPosition();
		long last = robot.getLeftWheelPosition();
		int stuckChecker = 0;
		int direction = (distance > 0) ? 1 : -1;
		  
		// move robot forward
		robot.setMotorSpeeds(speed * direction, speed * direction);
		while(Math.abs(robot.getLeftWheelPosition() - initialLeftWheelPosition) < Math.abs(distance)
				&& (canMoveForward() || forceMove)) {
			robot.sleep(1);
			if(stuckChecker % 1000 == 0) {
				last = robot.getLeftWheelPosition();
			} else if(stuckChecker % 1000 == 100 && last == robot.getLeftWheelPosition()) {
				System.err.println("Stuck during move " + ((direction == 1) ? "forward" : "backward"));
				robot.setMotorSpeeds(direction * -1, direction * -1);
				robot.sleep(200);
				break;
			}
			stuckChecker++;
		}

		// in case of moving too much, move slowly back
		robot.setMotorSpeeds(direction * -1, direction * -1);
		while((robot.getLeftWheelPosition() - initialLeftWheelPosition) * direction > distance * direction) {
			robot.sleep(1);
		}
		
		robot.setMotorSpeeds(0, 0);

		// change position of robot
		int moved = (int)Math.abs(robot.getLeftWheelPosition() - initialLeftWheelPosition);
		int orientation = (direction == 1) ? mapBuilder.getOrientation() : mapBuilder.getOrientation() + 180;
		Point newPosition = mapBuilder.getPosition().getAnother(orientation, moved);
		mapBuilder.setPosition(newPosition);
		
		return robot.getLeftWheelPosition() - initialLeftWheelPosition == distance;
	}
	
	/**
	 * Turns robot.
	 * @param angle		angle to turn
	 * @param speed		speed of turning (1 - 9)
	 * @return			true if required angle was turned, false otherwise
	 */
	public boolean turn(int degrees, int speed) {
		
		// check given parameters
		// TODO: change exception class
		if(speed < 1 || speed > 9) {
			throw new IllegalArgumentException("Speed " + speed + " is not allowed");
		}
		
		// degrees is value from (-180, 180)
		degrees = degrees % 360;
		if(degrees > 180) {
			degrees = degrees - 360;
		}
		
		// set direction of turning according given angle
		int direction = (degrees > 0) ? 1 : -1;
		int initialOrientation = mapBuilder.getOrientation();
		long initialRightWheelPosition = robot.getRightWheelPosition();
		long last = robot.getRightWheelPosition();
		int stuckChecker = 0;
		  
		// turning
		robot.setMotorSpeeds(speed * direction * -1, speed * direction);
		while(Math.abs(robot.getRightWheelPosition() - initialRightWheelPosition) < Math.abs(degrees * 3)) {
			robot.sleep(1);
			if(stuckChecker % 1000 == 0) {
				last = robot.getRightWheelPosition();
			} else if(stuckChecker % 1000 == 100 && last == robot.getRightWheelPosition()) {
				System.err.println("Stuck during turning");
				break;
			}
			stuckChecker++;
		}
		
		// in case of turning too much, turn slowly back
		robot.setMotorSpeeds(direction, direction * -1);
		while(Math.abs(robot.getRightWheelPosition() - initialRightWheelPosition) > Math.abs(degrees * 3)) {
			robot.sleep(1);
		}
			
		robot.setMotorSpeeds(0, 0);
		
		// change orientation of robot
		int realDegrees = (int)(robot.getRightWheelPosition() - initialRightWheelPosition) / 3;
		mapBuilder.setOrientation(mapBuilder.getOrientation() + realDegrees);
		
		// in case of getting stuck
		if (realDegrees != degrees) {
			long turned = robot.getRightWheelPosition() - initialRightWheelPosition;
			if(!moveBackward(30, 1)) {
				moveForward(30, 1);
			}
			initialRightWheelPosition = robot.getRightWheelPosition();
			robot.setMotorSpeeds(speed * direction, speed * direction * -1);
			while(Math.abs(robot.getRightWheelPosition() - initialRightWheelPosition) < Math.abs(turned)) {
				robot.sleep(1);
			}
			robot.setMotorSpeeds(0, 0);
			mapBuilder.setOrientation(initialOrientation);
			return false;
		} else {
			return true;
		}
	}

	/**
	 * turns robot to the ball
	 * @return	turned degree
	 */
	public int turnToBall() {
		int turned = 0;
		long initialOrientation = mapBuilder.getOrientation();
		int direction = (robot.getDistanceValue(1) + robot.getDistanceValue(2) - robot.getDistanceValue(0) > 
		    			 robot.getDistanceValue(3) + robot.getDistanceValue(4) - robot.getDistanceValue(5)) ? 1 : -1; // 1 left, -1 right
		while (robot.getDistanceValue(2) * direction > robot.getDistanceValue(3) * direction ||
				robot.getDistanceValue(2) + robot.getDistanceValue(3) < 20) {
			if((initialOrientation + 180) % 360 == mapBuilder.getOrientation() || !turn(direction, 1)) {
				turn(turned * -1, 1);
				turned = 0;
				break;
			}
			turned = turned + direction;
		}
		return turned;
	}
	
	/**
	 * turns robot to the light
	 * @return	turned degree
	 */
	public int turnToLight() {
		int turned = 0;
		int direction = (robot.getLightValue(1) + robot.getLightValue(2) <
				robot.getLightValue(3) + robot.getLightValue(4)) ? 1 : -1; // 1 left, -1 right
		while ((robot.getLightValue(1) + robot.getLightValue(2)) * direction <
			   (robot.getLightValue(3) + robot.getLightValue(4)) * direction) {
			if(!turn(direction, 1)) {
				turn(turned * -1, 1);
				turned = 0;
				break;
			}	
			turned = turned + direction;
		}
		return turned;
	}
	
	/**
	 * Moves robot directly to point
	 * @param end	point where to move
	 * @return		true if end position was reached, false otherwise
	 */
	public boolean moveDirectlyTo(Point end) {
		Point position = mapBuilder.getPosition();
		
		if(position.getX() < end.getX()) {
			turn(mapBuilder.getOrientation() * -1, SPEED);
			moveForward(end.getX() - position.getX(), SPEED, true);
		} else if(position.getX() > end.getX()){
			turn(mapBuilder.getOrientation() * -1 + 180, SPEED);
			moveForward(position.getX() - end.getX(), SPEED, true);
		}
		
		if(position.getY() < end.getY()) {
			turn(mapBuilder.getOrientation() * -1 + 90, SPEED);
			moveForward(end.getY() - position.getY(), SPEED, true);
		} else if(position.getY() > end.getY()){
			turn(mapBuilder.getOrientation() * -1 - 90, SPEED);
			moveForward(position.getY() - end.getY(), SPEED, true);
		}
		
		return mapBuilder.getPosition().equals(end);
	}

	
	/**
	 * Finds way and moves robot to point in map.
	 * @param end	point where to move
	 * @return		true if end position was reached, false otherwise
	 */
	public boolean moveTo(Point end) {
		List<Point> way;
		try {
			way = mapBuilder.findWay(end);
		} catch (NavigationException e) {
			return false;
		}
		for(Point crossRoad : way) {
			if(!moveDirectlyTo(crossRoad)) {
				System.err.println("Move directly " + crossRoad + " wasn't successful");
			}
		}
		return mapBuilder.getPosition().equals(end);
	}
}

